#include "Reader.h"

#define DEBUG 0

/**
 * @brief 异或校验
 */
unsigned char bcc(unsigned char *data, int len) {
    unsigned char res = 0;
    for (int i = 0; i < len; i++) {
        res ^= data[i];
    }
    return res;
}

/**
* @brief 格式化数组
*/
string bytes_2_hex(unsigned char data[], int length) {
    string hex;
    char ch[3] = {0};
    for (int i = 0; i < length; i++) {
        sprintf(ch, "%02X", data[i]);
        hex += ch;
    }
    return hex;
}

string bytes_2_hex2(unsigned char data[], int length) {
    string hex;
    char ch[4] = {0};
    for (int i = 0; i < length; i++) {
        sprintf(ch, "%02X ", data[i]);
        hex += ch;
    }
    return hex;
}

Reader::Reader() {
    memset(&jt_ret.data, 0, sizeof(jt_ret.data));
    jt_ret.cmd = 0;
    jt_ret.len = 0;
    memset(&jt_trans, 0, sizeof(JT_TRANS));
    read_thread_flag = false;
    sem_init(&jt_ret.semaphore, 0, 0);
    inventorying = false;
    flag_light_multi = false;
    high = 0;
    max_power = 33;
    max_power_times = 1;
}

Reader::~Reader() {
    sem_close(&jt_ret.semaphore);
    sem_destroy(&jt_ret.semaphore);
}

bool Reader::connect(const char *ip, short port) {
    int rc = 0;
    strncpy(this->ip, ip, sizeof(this->ip));
    this->port = port;
    if (socket2.open_s(ip, port) == RFID_SUCCESS) {
        /** 创建线程读取网络数据 */
        read_thread_flag = true;
        rc = pthread_create(&thread_read, NULL, ReadThreadProc, this);
        if (rc) {
            perror("Create ReadThread failed\n");
            return false;
        }
        return true;
    }
    return false;
}

bool Reader::disconnect() {
    if (read_thread_flag) {
        read_thread_flag = false;
        pthread_join(thread_read, NULL);
    }
    socket2.close_s();
    return true;
}

void *Reader::ReadThreadProc(void *args) {
    Reader *self = (Reader *) args;
    JNIEnv *env = NULL;
    CALLBACK_IDS ids;
    if (self->jvm == NULL) {
        perror("jvm is null\n");
        return 0;
    }

    if (self->jvm->GetEnv((void **) &env, JNI_VERSION_1_6) != JNI_OK) {
        int rc = self->jvm->AttachCurrentThread((void **)&env, NULL);
        if (rc < 0) {
            perror("ReadThread failed to attach current thread");
            return 0;
        }
        jclass cls = env->GetObjectClass(self->thiz);

        if (cls != NULL) {
            ids.on_alive = env->GetMethodID(
                    cls,
                    "onAlive",
                    "()V"
                    );
            ids.on_inv = env->GetMethodID(
                    cls,
                    "onInventory",
                    "(Ljava/lang/String;III)V");

            ids.on_inv_stop = env->GetMethodID(
                    cls,
                    "onStop",
                    "()V"
                    );
        }
    }
    while (self->read_thread_flag) {
        self->jt_trans.size = self->socket2.read_s((char *) self->jt_trans.data, MAX_READ_LENGTH);
        if (self->jt_trans.size > 0 && self->jt_trans.size < MAX_READ_LENGTH) {
            if (DEBUG) {
                std::string hex = bytes_2_hex2(self->jt_trans.data, self->jt_trans.size);
                printf("<<< %s\n", hex.c_str());
            }

            self->translate(env, ids);
        }
        usleep(10 * 1000);
    }
    printf("ReaderThread exit\n");
    return 0;
}

void Reader::translate(JNIEnv *env, CALLBACK_IDS ids) {
    unsigned char b;
    for (int i = 0; i < jt_trans.size; i++) {
        b = jt_trans.data[i];
        if (jt_trans.head_count < HEADER_LEN) {
            switch (jt_trans.head_count) {
                case 0:
                    if (STX1 == b) {
                        jt_trans.head_count++;
                    } else {
                        jt_trans.head_count = 0;
                    }
                    break;
                case 1:
                    if (STX2 == b) {
                        jt_trans.head_count++;
                    } else {
                        jt_trans.head_count = 0;
                    }
                    break;
                case 2:
                    jt_trans.cmd = b;
                    jt_trans.head_count++;
                    break;
                case 3:
                    if (STX_SLAVE_2_HOST == b) {
                        jt_trans.head_count++;
                    } else {
                        jt_trans.head_count = 0;
                    }
                    break;
                case 4:
                    jt_trans.payload_len = b;
                    jt_trans.head_count++;
                    break;
                case 5:
                    jt_trans.payload_len += (b << 8);
                    if (jt_trans.payload_len > MAX_READ_LENGTH) {
                        jt_trans.head_count = 0;
                    } else {
                        jt_trans.data_count = 0;
                        jt_trans.bcc = 0;
                        jt_trans.payload_len -= 2;
                        jt_trans.head_count++;
                    }
                    break;
                case 6:
                    jt_trans.id = b;
                    jt_trans.head_count++;
                    jt_trans.bcc ^= b;
                    break;
                case 7:
                    jt_trans.id += (b << 8);
                    jt_trans.head_count++;
                    jt_trans.bcc ^= b;
                    break;
                default:
                    break;
            }
        } else {
            if (jt_trans.data_count < jt_trans.payload_len) {
                jt_trans.payload[jt_trans.data_count++] = b;
                jt_trans.bcc ^= b;
            } else {
                if (jt_trans.bcc == b) {
                    command(env, ids);
                }
                jt_trans.data_count = 0;
                jt_trans.head_count = 0;
            }
        }
    }
}

void Reader::command(JNIEnv *env, CALLBACK_IDS ids) {
    switch (jt_trans.cmd) {
        case CMD_ALIVE:
            if (ids.on_alive != NULL) {
                env->CallVoidMethod(thiz, ids.on_alive);
            }
            break;
        case CMD_INV_ONCE:
            if (jt_trans.payload_len == 1 && jt_trans.payload[0] == 0) {
                sem_post(&jt_ret.semaphore);
                if (DEBUG) {
                    printf("Inv once begin\n");
                }
                this->inventorying = true;
            }
            else if (jt_trans.payload_len == 2 && jt_trans.payload[0] == 0 && jt_trans.payload[1] == 3) {
                if (DEBUG) {
                    printf("Inv once stop\n");
                }
                
                ++inv_completed;
                if (inv_completed == inv_times) {
                    this->inventorying = false;
                    if (ids.on_inv_stop != NULL) {
                        env->CallVoidMethod(thiz, ids.on_inv_stop);
                    }
                    printf("Inv once completed\n");
                } else {
                    // 发送盘点一次的指令
                    // 最后的几次以功率最大盘点
                    if (inv_completed == (inv_times - max_power_times - 1)) {
                        pthread_create(&thread_ant_inv, NULL, SetAntAndInvProc, this);
                    } else {
                        write_cmd(CMD_INV_ONCE, NULL, 0);
                    }
                }                
            }
            else if (jt_trans.payload_len > 2 && jt_trans.payload[0] == 0x08) {
                int epc_len = jt_trans.payload[1];
                if (jt_trans.payload_len == epc_len + 5) {
                    string epc = bytes_2_hex(&jt_trans.payload[3], epc_len);
                    int ant = jt_trans.payload[3 + epc_len];
                    short rssi = 0 - jt_trans.payload[4 + epc_len];
                    if (ids.on_inv != NULL) {
                        jstring jepc = std2j(env, epc.c_str());
                        env->CallVoidMethod(thiz, ids.on_inv, jepc, ant, rssi, this->high);
                        env->ReleaseStringChars(jepc,
                                                env->GetStringChars(jepc, JNI_FALSE));
                        env->DeleteLocalRef(jepc);
                    }
                }
            }
            break;
        case CMD_GET_ANT:
        case CMD_SET_ANT:
            if (jt_ret.cmd == jt_trans.cmd) {
                jt_ret.len = jt_trans.payload_len;
                if (jt_ret.len > 0) {
                    memcpy(jt_ret.data, &jt_trans.payload[0], jt_ret.len);
                }
                sem_post(&jt_ret.semaphore);
            }
            break;
        default:
            break;
    }
}

int Reader::write_cmd(unsigned char cmd, unsigned char *data, unsigned short len) {
    unsigned char *send = new unsigned char[len + HEADER_LEN + 1]();
    send[0] = STX1;
    send[1] = STX2;
    send[2] = cmd;
    send[3] = STX_HOST_2_SLAVE;
    send[4] = len + 2;
    send[5] = (len + 2) / 256;
    send[6] = 0;
    send[7] = 0;
    if (len > 0) {
        memcpy(&send[8], data, len);
    }
    send[8 + len] = bcc(data, len);
    if (DEBUG) {
        std::string hex = bytes_2_hex2(send, len + HEADER_LEN + 1);
        printf(">>> %s\n", hex.c_str());
    }
    int rc = socket2.write_s((char *) send, len + HEADER_LEN + 1);
    delete[] send; send = NULL;
    return rc > 0 ? RFID_SUCCESS : RFID_FAIL;
}

int Reader::read_cmd(unsigned char cmd, unsigned int timeout) {
    jt_ret.data[0] = RFID_FAIL;
    jt_ret.cmd = cmd;
    sem_init(&jt_ret.semaphore, 0, 0);
    //printf("read_cmd\n");
    struct timespec ts = sem_add_ms(timeout);
    int iRet = sem_timedwait(&jt_ret.semaphore, &ts);
    return iRet > -1 ? RFID_SUCCESS : RFID_FAIL;
}

int Reader::inventory(int times) {
    if (inventorying) return RFID_FAIL;
    stop_light_multi();
    this->inv_times = times;
    this->inv_completed = 0;
    this->high = 0;
    if (write_cmd(CMD_INV_ONCE, NULL, 0) == RFID_SUCCESS) {
        return read_cmd(CMD_INV_ONCE);
    }
    return RFID_FAIL;
}

int Reader::light(unsigned char *epc, unsigned char len) {
    if (inventorying) return RFID_FAIL;
    unsigned  char *data = new unsigned char[len + 11];
    data[0] = 0; // 标准模式
    data[1] = 0; // 保留
    data[2] = len * 8;
    if (len > 0) {
        memcpy(&data[3], epc, len);
    }
    data[3 + len] = 0;
    data[4 + len] = 0;
    data[5 + len] = 0;
    data[6 + len] = 0; // 访问密码
    data[7 + len] = 0; // reserve区
    data[8 + len] = 0;
    data[9 + len] = 4; // 偏移量
    data[10 + len] = 1; // 读取长度为1
    int res = RFID_FAIL;
    if (write_cmd(CMD_SEL_READ, data, len + 11) == RFID_SUCCESS) {
        if (read_cmd(CMD_SEL_READ, 50) == RFID_SUCCESS) {
            res = jt_ret.data[0];
        }
    }
    delete[] data; data = NULL;
    return res;
}

int Reader::light2(unsigned char *epc, unsigned char len) {
    if (inventorying) return RFID_FAIL;
    unsigned  char *data = new unsigned char[len + 11];
    data[0] = 0; // 标准模式
    data[1] = 0; // 保留
    data[2] = len * 8;
    if (len > 0) {
        memcpy(&data[3], epc, len);
    }
    data[3 + len] = 0;
    data[4 + len] = 0;
    data[5 + len] = 0;
    data[6 + len] = 0; // 访问密码
    data[7 + len] = 0; // reserve区
    data[8 + len] = 0;
    data[9 + len] = 4; // 偏移量
    data[10 + len] = 1; // 读取长度为1
    int res = RFID_FAIL;
    if (write_cmd(CMD_SEL_READ, data, len + 11) == RFID_SUCCESS) {
        if (read_cmd(CMD_SEL_READ, 150) == RFID_SUCCESS) {
            res = jt_ret.data[0];
        }
    }
    delete[] data; data = NULL;
    return res;
}

int Reader::get_ant(JT_ANT_16 &ant) {
    if (write_cmd(CMD_GET_ANT, NULL, 0) == RFID_SUCCESS) {
        if (read_cmd(CMD_GET_ANT) == RFID_SUCCESS) {
            memcpy(&ant, jt_ret.data, jt_ret.len);
            return RFID_SUCCESS;
        }
    }
    return RFID_FAIL;
}

int Reader::set_ant(JT_ANT_16 ant) {
    for (int i = 0; i < 3; i++) {
        if (write_cmd(CMD_SET_ANT, (unsigned char *)&ant, sizeof(JT_ANT_16)) == RFID_SUCCESS) {
            if (read_cmd(CMD_SET_ANT) == RFID_SUCCESS) {
                return jt_ret.data[0];
            }
        }
    }
    return RFID_FAIL;
}

int Reader::start_light_multi(std::list<SEL_MASK> marks, int times) {
    if (inventorying || flag_light_multi) return RFID_FAIL;
    flag_light_multi = true;

    this->marks.clear();
    std::list<SEL_MASK>::iterator it;
    for (it = marks.begin(); it != marks.end(); ++it) {
        this->marks.push_back(*it);
    }
    this->light_times = times;
    int rc = pthread_create(&thread_light_multi, NULL, LightMultiProc2, this);
    if (rc) {
        perror("Create LightMulti thread failed\n");
        flag_light_multi = true;
        return RFID_FAIL;
    }
    return RFID_SUCCESS;
}

void *Reader::LightMultiProc(void *args) {
    Reader *self = (Reader *) args;
    std::list<SEL_MASK>::iterator  it;
    std::list<int> ants;
    std::list<int>::iterator  itt;
    for (it = self->marks.begin(); it != self->marks.end(); ++it) {
        itt = find(ants.begin(), ants.end(), it->ant);
        if (itt == ants.end()) {
            ants.push_back(it->ant);
        }
    }
    while (self->flag_light_multi) {
        for (itt = ants.begin(); itt != ants.end(); ++itt) {
            JT_ANT_16 ant16;
            ant16.store = 0;
            ant16.count = 16;
            ant16.enable = 1 << (*itt - 1);
            for (int i = 0; i < 16; i++) {
                ant16.ants[i].idx = i + 1;
                ant16.ants[i].power = 33;
                ant16.ants[i].work_ms = htons(200);
                ant16.ants[i].sleep_ms = htons(100);
            }
            for (it = self->marks.begin(); it != self->marks.end(); ++it) {
                if (!self->flag_light_multi) break;
                self->set_ant(ant16);
                if (it->ant == *itt) {
                    for (int i = 0; i < self->light_times; i++) {
                        if (!self->flag_light_multi) break;
                        self->light(it->mask, it->len);
                    }
                }
            }
        }
    }
    return 0;
}

void *Reader::LightMultiProc2(void *args) {
    Reader *self = (Reader *) args;
    std::list<SEL_MASK>::iterator  it;
    std::list<unsigned short> channels;
    std::list<unsigned short>::iterator  itt;
    for (it = self->marks.begin(); it != self->marks.end(); ++it) {
        itt = find(channels.begin(), channels.end(), it->ant);
        if (itt == channels.end()) {
            channels.push_back(it->channels);
        }
    }
    while (self->flag_light_multi) {
        for (itt = channels.begin(); itt != channels.end(); ++itt) {
            JT_ANT_16 ant16;
            ant16.store = 0;
            ant16.count = 16;
            ant16.enable = *itt;
            for (int i = 0; i < 16; i++) {
                ant16.ants[i].idx = i + 1;
                ant16.ants[i].power = 33;
                ant16.ants[i].work_ms = htons(50);
                ant16.ants[i].sleep_ms = htons(0);
            }
            for (it = self->marks.begin(); it != self->marks.end(); ++it) {
                if (!self->flag_light_multi) break;
                self->set_ant(ant16);
                if (it->channels == *itt) {
                    for (int i = 0; i < self->light_times; i++) {
                        if (!self->flag_light_multi) break;
                        self->light2(it->mask, it->len);
                    }
                }
            }
        }
    }
    return 0;
}


int Reader::stop_light_multi() {
    if (flag_light_multi) {
        flag_light_multi = false;
        pthread_join(thread_light_multi, NULL);
    }
    return RFID_SUCCESS;
}

void *Reader::SetAntAndInvProc(void *args) {
    Reader *self = (Reader *)args;
    JT_ANT_16 jt_ant;
    if (self->get_ant(jt_ant) == RFID_SUCCESS) {
        for (int i = 0; i < 16; i++) {
            jt_ant.ants[i].power = self->max_power;
        }
        self->set_ant(jt_ant);
        self->high = 1;
        printf("Set Ant Max Power\n");
    }
    self->write_cmd(CMD_INV_ONCE, 0, 0);
    return 0;
}
void Reader::set_multi_param(int maxPower, int maxPowerTimes) {
    this->max_power = maxPower;
    this->max_power_times = maxPowerTimes;
}